#pragma once

#include "interface/controller.h"
#include "interface/common.h"

#include <string>
#include <cmath>
#include <thread>
#include <yaml-cpp/yaml.h>

/**
 * 路径跟踪控制器类，用于控制车辆沿预定路径行驶
 */
class PathFollowController : public ControllerInterface {
public:
    PathFollowController() : follow_state_(FollowState::kInitializing) {
        std::string yaml_file = std::string(CMAKE_LISTS_DIR) + "/config/slim_params.yaml";
        YAML::Node  config    = YAML::LoadFile(yaml_file);

        wheel_base_      = config["wheel_base"].as<float>();
        max_linear_vel_  = config["max_linear_vel"].as<float>();
        min_linear_vel_  = config["min_linear_vel"].as<float>();
        max_steer_angle_ = config["max_steer_angle"].as<float>();
        control_period_  = config["control_period"].as<float>();

        std::cout << "wheel_base_:" << wheel_base_ << std::endl;
        std::cout << "max_linear_vel_:" << max_linear_vel_ << std::endl;
        std::cout << "min_linear_vel_:" << min_linear_vel_ << std::endl;
        std::cout << "max_steer_angle_:" << max_steer_angle_ << std::endl;
        std::cout << "control_period_:" << control_period_ << std::endl;

        follow_state_ = FollowState::kIdle;
    }

    // 获取输出的线速度和转向角
    void getOut(float &lianer_vel, float &steer_angle) override;

    void startFollow(std::vector<Point> path) override;

    FollowState getState() override;

    /**
     * 计算转向角的纯虚函数，需要由子类实现
     * @param lookahead_idx 预瞄点索引
     * @return 转向角度
     */
    virtual float CalcSteeringAngle(int lookahead_idx) = 0;

    /**
     * 根据转向角计算线速度的纯虚函数，需要由子类实现
     * @param steer_angle 转向角度
     * @return 线速度
     */
    virtual float CalcLinearVel(float steer_angle) = 0;

    /**
     * 路径跟随线程，内容需要由子类实现
     */

    virtual void FollowThread() = 0;
    /**
     * 查找路径上最接近当前车辆位置的点的索引
     * @return 最接近点的索引
     */
    int FindClosestPoint() {
        size_t closest_idx = 0;
        double min_dist    = std::numeric_limits<double>::max();

        for (size_t i = 0; i < path_.size(); ++i) {
            double dx   = path_[i].x - pose_.x;
            double dy   = path_[i].y - pose_.y;
            double dist = std::sqrt(dx * dx + dy * dy);

            if (dist < min_dist) {
                min_dist    = dist;
                closest_idx = i;
            }
        }

        return closest_idx;
    }

    // 判断是否到达目标点
    bool isReachGoal() {
        float dx   = pose_.x - path_.back().x;
        float dy   = pose_.y - path_.back().y;
        float dist = std::sqrt(dx * dx + dy * dy);
        if (dist < 0.1) {
            std::cout << "reach goal!" << std::endl;
            return true;
        }

        return false;
    };

protected:
    std::vector<Point> path_;  // 路径
    Odometry           pose_;  // 位姿

    float wheel_base_;       // 轴距
    float max_linear_vel_;   // 最大线速度
    float min_linear_vel_;   // 最小线速度
    float max_steer_angle_;  // 最大转角
    float control_period_;   // 控制周期(s)

    float linear_vel_;   // 输出线速度
    float steer_angle_;  // 输出转角

    // 路径跟随中的状态
    FollowState follow_state_;
};